#include "Controller.hpp"

#include <opencv4/opencv2/opencv.hpp>
#include <opencv4/opencv2/cudaimgproc.hpp>

#include <pthread.h>
#include <iostream>
#include <thread>
#include <boost/filesystem.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

#include <SerialPortUtilities/BytesAccessor.hpp>
#include <SerialPortUtilities/GeneralMessageTranslator.hpp>
#include <SerialPortUtilities/TurretCommand.pb.hpp>
#include <SerialPortUtilities/Signal.pb.hpp>

namespace RoboPioneers::Prometheus
{
    /// 设置当前线程的线程亲和度
    void SetCurrentThreadCPUAffinity(const std::vector<unsigned int>& cpus)
    {
        cpu_set_t mask;

        CPU_ZERO(&mask);

        for (const auto index : cpus)
        {
            CPU_SET(index, &mask);
        }
        sched_setaffinity(0, sizeof(mask), &mask);
    }

    /// 执行方法
    void Controller::Launch()
    {

        SerialConnection.Open();

        //todo read content
        while (false)
        {
            std::vector<unsigned char> buffer;
            while(true)
            {
                buffer = SerialConnection.ReadBytes(-1);
//                buffer.push_back(1);
                std::cout << "buffer[0] Receive: " << static_cast<unsigned int>(buffer[0]) << std::endl;
                if (static_cast<unsigned int>(buffer[0]) != 0)
                {
                    break;
                }
            }
            Signal team_color;
            team_color.ParseFromArray(&buffer, buffer.size());
            if (team_color.name() == "sc_r")
            {
                EnemyColor = ColorType::Blue;
                std::cout << "Enemy Color: Blue" << std::endl;
                break;
            }
            else
            {
                EnemyColor = ColorType::Red;
                std::cout << "Enemy Color: Red" << std::endl;
                break;
            }
        }

        OnInstall();

        std::this_thread::sleep_for(std::chrono::seconds(2));

        Cameras::Galaxy::RawPicture raw_picture;

        try
        {
            while (true)
            {
#ifdef DEBUG
                // 等待按下ASCII为27的键（ESC键），按下则终止程序
                if (cv::waitKey(1) == 27)
                {
                    /// JSON 存储
                    {
                        boost::property_tree::ptree json_node;

                        json_node.put<int>("Mask.Red.Hue.Min", ColorStage.MinHue);
                        json_node.put<int>("Mask.Red.Hue.Max", ColorStage.MaxHue);
                        json_node.put<int>("Mask.Red.Saturation.Min", ColorStage.MinSaturation);
                        json_node.put<int>("Mask.Red.Saturation.Max", ColorStage.MaxSaturation);
                        json_node.put<int>("Mask.Red.Value.Min", ColorStage.MinValue);
                        json_node.put<int>("Mask.Red.Value.Max", ColorStage.MaxValue);

                        json_node.put<int>("LightBar.MinArea", LightBarStage.MinArea);
                        json_node.put<int>("LightBar.MinFillingRatio", LightBarStage.MinFillingRatio);
                        json_node.put<int>("LightBar.MaxAngleDifference", ArmorStage.MaxAngleDifference);
                        json_node.put<int>("LightBar.DeltaYHeightRatio.Max", ArmorStage.MaxDeltaYHeightRatio);

                        json_node.put<int>("BigArmor.HeightDistanceRatio.Min", ArmorStage.MinHeightDistanceRatioBigArmor);
                        json_node.put<int>("BigArmor.HeightDistanceRatio.Max", ArmorStage.MaxHeightDistanceRatioBigArmor);
                        json_node.put<int>("BigArmor.WidthDistanceRatio.Min", ArmorStage.MinWidthDistanceRatioBigArmor);
                        json_node.put<int>("BigArmor.WidthDistanceRatio.Max", ArmorStage.MaxWidthDistanceRatioBigArmor);

                        json_node.put<int>("SmallArmor.HeightDistanceRatio.Min", ArmorStage.MinHeightDistanceRatioSmallArmor);
                        json_node.put<int>("SmallArmor.HeightDistanceRatio.Max", ArmorStage.MaxHeightDistanceRatioSmallArmor);
                        json_node.put<int>("SmallArmor.WidthDistanceRatio.Min", ArmorStage.MinWidthDistanceRatioSmallArmor);
                        json_node.put<int>("SmallArmor.WidthDistanceRatio.Max", ArmorStage.MaxWidthDistanceRatioSmallArmor);

                        boost::property_tree::write_json("Settings.json", json_node);
                    }
                    break;
                }
#endif

                /* 此处OpenCV可能会抛出异常，故加上该try块，以跳过因通信故障导致异常的帧
                 */

#ifdef DEBUG
                cv::namedWindow("Parameter Panel");
                // Color
                cv::createTrackbar("HueMin","Parameter Panel",&ColorStage.MinHue,255);
                cv::createTrackbar("HumMax","Parameter Panel",&ColorStage.MaxHue,255);
                cv::createTrackbar("SaturationMin","Parameter Panel",&ColorStage.MinSaturation,255);
                cv::createTrackbar("SaturationMax","Parameter Panel",&ColorStage.MaxSaturation,255);
                cv::createTrackbar("ValueMin","Parameter Panel",&ColorStage.MinValue,255);
                cv::createTrackbar("ValueMax","Parameter Panel",&ColorStage.MaxValue,255);
                // Light Bar
                cv::createTrackbar("LightMinArea","Parameter Panel",&LightBarStage.MinArea,300);
                cv::createTrackbar("LightMinFillRatio","Parameter Panel",&LightBarStage.MinFillingRatio,100);
                cv::createTrackbar("LightBar.MaxAngleDifference","Parameter Panel",&ArmorStage.MaxAngleDifference,180);
                cv::createTrackbar("LightMinArea","Parameter Panel",&ArmorStage.MaxDeltaYHeightRatio,100);
                // Armor
                cv::createTrackbar("MinHeightDistanceRatio","Parameter Panel",&ArmorStage.MinHeightDistanceRatioBigArmor,100);
                cv::createTrackbar("MaxHeightDistanceRatio","Parameter Panel",&ArmorStage.MaxHeightDistanceRatioBigArmor,100);
                cv::createTrackbar("MinWidthDistanceRatio","Parameter Panel",&ArmorStage.MinWidthDistanceRatioBigArmor,100);
                cv::createTrackbar("MaxWidthDistanceRatio","Parameter Panel",&ArmorStage.MaxWidthDistanceRatioBigArmor,100);
                ArmorStage.MinHeightDistanceRatioSmallArmor = ArmorStage.MinHeightDistanceRatioBigArmor;
                ArmorStage.MaxHeightDistanceRatioSmallArmor = ArmorStage.MaxHeightDistanceRatioBigArmor;
                ArmorStage.MinWidthDistanceRatioSmallArmor = ArmorStage.MinWidthDistanceRatioBigArmor;
                ArmorStage.MaxWidthDistanceRatioSmallArmor = ArmorStage.MaxWidthDistanceRatioBigArmor;

#endif

                StartTime = std::chrono::steady_clock::now();

                raw_picture = Camera.GetCurrentPicture();

                cv::Mat original_picture(cv::Size(raw_picture.Width, raw_picture.Height), CV_8UC1, raw_picture.Data);
                cv::cvtColor(original_picture, original_picture, cv::COLOR_BayerBG2BGR);

                // 相机图像反转
                if (IsRequiredFlip==true)
                {
                    cv::flip(original_picture, original_picture, -1);
                }

#ifdef DEBUG
                cv::imshow("ColorFilter",original_picture);

#endif

                CuttingStage.OriginalPicture = &original_picture;
                CuttingStage.Execute();
#ifdef DEBUG
                cv::imshow("Cutting Result", CuttingStage.CuttingPicture);
#endif

#ifdef DEBUG

#endif

                ColorStage.BGRPicture = CuttingStage.CuttingPicture;
                ColorStage.Execute();

#ifdef DEBUG

                cv::imshow("Color Binary", ColorStage.BinaryPicture);

#endif
                LightBarStage.Execute();
                ArmorStage.Execute();
                RecommendStage.Execute();


#ifdef DEBUG
                cv::Mat target = original_picture.clone();
                cv::rectangle(target,RecommendStage.ArmorBox,cv::Scalar(0,255,0));
                cv::imshow("armor",target);
                if (RecommendStage.Found)
                {
                    // 输出坐标
                   std::cout << "Found X:" << RecommendStage.X << " Y:" << RecommendStage.Y
                   << " Distance:"<< RecommendStage.Distance << "cm" << std::endl;
                }
#endif

                // 旧版串口发送模块 准备用于传输的字节包
//                std::vector<unsigned char> bytes;
//                bytes.resize(13);
//                SerialPort::Utilities::BytesAccessor accessor(bytes.data(), 13);
//                accessor.Access<unsigned char>(0) = 0xFF;
//                if (RecommendStage.Found )
//                {
//                    accessor.Access<unsigned char>(1) = 1;
//                } else
//                {
//                    accessor.Access<unsigned char>(1) = 0;
//                }
//                accessor.Access<unsigned short>(2) = static_cast<unsigned short>(RecommendStage.X);
//                accessor.Access<unsigned short>(4) = static_cast<unsigned short>(RecommendStage.Y);
//                accessor.Access<unsigned short>(6) = static_cast<unsigned short>(RecommendStage.Distance);
//                accessor.Access<unsigned char>(8) =
//                        SerialPort::Utilities::CRCTool::GetCRC8CheckSum(bytes.data(), 8);

                //todo Open SerialPort Write
                TurretCommand command;
                command.set_yaw(static_cast<float>(RecommendStage.X));
                command.set_pitch(static_cast<float>(RecommendStage.Y));
                command.set_diatance(static_cast<float>(RecommendStage.Distance));
                command.set_command(RecommendStage.Found);

                std::string command_bytes;
                command_bytes.resize(command.ByteSize());
                command.SerializeToArray(command_bytes.data(), static_cast<int>(command_bytes.size()));

                std::string package_bytes;
                package_bytes = SerialPort::Utilities::GeneralMessageTranslator::Encode(3,command_bytes);

                // 传输字节包
                SerialConnection.Write(package_bytes);
                FPSStage.Execute();

                // 锁定帧率以固定时间周期发送
                auto duration = std::chrono::steady_clock::now() - StartTime;
                auto rest_time = 1000*1000/SendingFrequency
                                 - std::chrono::duration_cast<std::chrono::microseconds>(duration).count();
                if(rest_time>=0)
                {
                    std::this_thread::sleep_for(std::chrono::microseconds(rest_time));
                }


            }
        }
        catch (cv::Exception& error)
        {
            std::cout << "Exception Occurs: " << error.what() << std::endl;
        }
        OnUninstall();
    }

    /// 安装方法
    void Controller::OnInstall()
    {
        OnLoadConfiguration();
        /*
         * 允许当前线程及子线程覆盖全部的CPU核心
         * 对于JetPack4.4而言，两个Denver大核因为执行延迟问题被单独隔离，需要手动指定在其上工作的线程
         */
        SetCurrentThreadCPUAffinity({0,1,2,3,4,5});

        while(!Camera.IsOpened())
        {
            try {
                Camera.Open();
                Camera.SetExposureTime(1000);
                Camera.SetGain(0);
                Camera.SetAutoWhiteBalance();
            }catch(std::exception& error)
            {
                std::cout << "[Error] Failed to Open Camera: '" << error.what()
                          << "', will Attempt in 10 Seconds." << std::endl;
                std::this_thread::sleep_for(std::chrono::seconds(3));
            }
        }

        CuttingStage.InterestedArea = &RecommendStage.InterestedArea;
        CuttingStage.Found = &RecommendStage.Found;
        LightBarStage.BinaryPicture = &ColorStage.BinaryPicture;
        ArmorStage.LightBars = &LightBarStage.LightBars;
        RecommendStage.Armors = &ArmorStage.Armors;
        RecommendStage.PositionOffset = &CuttingStage.PositionOffset;
        FPSStage.Found = &RecommendStage.Found;

        SendingFrequency = 80;
    }

    /// 卸载方法
    void Controller::OnUninstall()
    {
        Camera.Close();

#ifndef DEBUG
        SerialConnection.Close();
#endif
    }

    /// 构造函数
    Controller::Controller() : Camera(0), SerialConnection("/dev/ttyTHS2")
    {}

    /// 加载配置文件
    void Controller::OnLoadConfiguration()
    {
        // 确保日志路径存在
        if(boost::filesystem::exists("Settings.json"))
        {
            boost::property_tree::ptree json_node;
            boost::property_tree::read_json("Settings.json", json_node);

            if (EnemyColor == ColorType::Red)
            {
                ColorStage.MinHue = json_node.get<int>("Mask.Red.Hue.Min");
                ColorStage.MaxHue = json_node.get<int>("Mask.Red.Hue.Max");
                ColorStage.MinSaturation = json_node.get<int>("Mask.Red.Saturation.Min");
                ColorStage.MaxSaturation = json_node.get<int>("Mask.Red.Saturation.Max");
                ColorStage.MinValue = json_node.get<int>("Mask.Red.Value.Min");
                ColorStage.MaxValue = json_node.get<int>("Mask.Red.Value.Max");
            }
            else
            {
                ColorStage.MinHue = json_node.get<int>("Mask.Blue.Hue.Min");
                ColorStage.MaxHue = json_node.get<int>("Mask.Blue.Hue.Max");
                ColorStage.MinSaturation = json_node.get<int>("Mask.Blue.Saturation.Min");
                ColorStage.MaxSaturation = json_node.get<int>("Mask.Blue.Saturation.Max");
                ColorStage.MinValue = json_node.get<int>("Mask.Blue.Value.Min");
                ColorStage.MaxValue = json_node.get<int>("Mask.Blue.Value.Max");
            }


            LightBarStage.MinArea = json_node.get<int>("LightBar.MinArea");
            LightBarStage.MinFillingRatio = json_node.get<int>("LightBar.MinFillingRatio");

            ArmorStage.MaxAngleDifference = json_node.get<int>("LightBar.MaxAngleDifference");
            ArmorStage.MaxDeltaYHeightRatio = json_node.get<int>("LightBar.DeltaYHeightRatio.Max");

            ArmorStage.MinHeightDistanceRatioBigArmor = json_node.get<int>("BigArmor.HeightDistanceRatio.Min");
            ArmorStage.MaxHeightDistanceRatioBigArmor = json_node.get<int>("BigArmor.HeightDistanceRatio.Max");
            ArmorStage.MinWidthDistanceRatioBigArmor = json_node.get<int>("BigArmor.WidthDistanceRatio.Min");
            ArmorStage.MaxWidthDistanceRatioBigArmor = json_node.get<int>("BigArmor.WidthDistanceRatio.Max");

            ArmorStage.MinHeightDistanceRatioSmallArmor = json_node.get<int>("SmallArmor.HeightDistanceRatio.Min");
            ArmorStage.MaxHeightDistanceRatioSmallArmor = json_node.get<int>("SmallArmor.HeightDistanceRatio.Max");
            ArmorStage.MinWidthDistanceRatioSmallArmor = json_node.get<int>("SmallArmor.WidthDistanceRatio.Min");
            ArmorStage.MaxWidthDistanceRatioSmallArmor = json_node.get<int>("SmallArmor.WidthDistanceRatio.Max");

            std::clog << "[Message] Using Settings in Settings.json." << std::endl;
        }
    }
}
